#include <cstdlib>
#include "Hough.h"
#include <math.h>
#include <stdio.h>
#include <iostream>
using namespace cv;
using namespace std;

double DEG2RAD = CV_PI/180;

//no hay round en math de compiladores de windows
inline double round( double d )
{
	return floor( d + 0.5 );
}

Hough::Hough():acumulador(0), anchoAcum(0), altoAcum(0), anchoImagen(0), altoImagen(0)
{
}
int Hough::Transform(vector<Point> contorno, int w, int h)  
{  
	anchoImagen = w;  
	altoImagen = h;  

	//Crea el arreglo acumulador  
	double hough_h = ((sqrt(2.0) * (double)(h > w ? h : w)) / 2.0);  
	altoAcum = hough_h * 2.0; // -r -> +r  
	anchoAcum = 180;  

	acumulador = (unsigned int*)malloc(altoAcum * anchoAcum*sizeof(unsigned int));  
	for (int i=0; i<altoAcum * anchoAcum;i++) 
		acumulador[i] = 0;
	double center_x = w/2;  
	double center_y = h/2;  


	for(size_t i = 0; i < contorno.size(); i++)  
	{  
		Point punto = contorno.at(i);
		double x = punto.x;
		double y = punto.y;
		for(int t=0;t<180;t++)  
		{  
			double r = ( (x - center_x) * cos((double)t * DEG2RAD)) + ((y - center_y) * sin((double)t * DEG2RAD));
			int indice = (int)((round(r + hough_h) * 180.0)) + t;
			acumulador[indice]=acumulador[indice]+1;  
		}  
	}  
	/*
	int aux = 1;
	for (int i=0; i<altoAcum * anchoAcum;i++) {
	aux = aux < acumulador[i] ? acumulador[i] : aux;
	}
	*/
	return 0;  
}  

vector<Vec4i> Hough::GetLines(int threshold)
{
	vector<Vec4i> lines;
	if(acumulador == 0)
		return lines;

	for(int r=0;r<altoAcum;r++)
	{
		for(int t=0;t<anchoAcum;t++)
		{
			if((int)acumulador[(r*anchoAcum) + t] >= threshold)
			{
				//Is this point a local maxima (9x9)
				int max = acumulador[(r*anchoAcum) + t];
				for(int ly=-4;ly<=4;ly++)
				{
					for(int lx=-4;lx<=4;lx++)
					{
						if( (ly+r>=0 && ly+r<altoAcum) && (lx+t>=0 && lx+t<anchoAcum)  )
						{
							if( (int)acumulador[( (r+ly)*anchoAcum) + (t+lx)] > max )
							{
								max = acumulador[( (r+ly)*anchoAcum) + (t+lx)];
								ly = lx = 5;
							}
						}
					}
				}
				if(max > (int)acumulador[(r*anchoAcum) + t])
					continue;


				int x1, y1, x2, y2;
				x1 = y1 = x2 = y2 = 0;

				if(t >= 45 && t <= 135)
				{
					if (t >= 83 && t <= 97){
						//y = (r - x cos(t)) / sin(t)
						x1 = 0;
						y1 = ((double)(r-(altoAcum/2)) - ((x1 - (anchoImagen/2) ) * cos(t * DEG2RAD))) / sin(t * DEG2RAD) + (altoImagen / 2);
						x2 = anchoImagen - 0;
						y2 = ((double)(r-(altoAcum/2)) - ((x2 - (anchoImagen/2) ) * cos(t * DEG2RAD))) / sin(t * DEG2RAD) + (altoImagen / 2);
					}else{
						continue;
					}
				}
				else
				{
					if (t > 7 && t < 173) 
						continue;
					//x = (r - y sin(t)) / cos(t);
					y1 = 0;
					x1 = ((double)(r-(altoAcum/2)) - ((y1 - (altoImagen/2) ) * sin(t * DEG2RAD))) / cos(t * DEG2RAD) + (anchoImagen / 2);
					y2 = altoImagen - 0;
					x2 = ((double)(r-(altoAcum/2)) - ((y2 - (altoImagen/2) ) * sin(t * DEG2RAD))) / cos(t * DEG2RAD) + (anchoImagen / 2);
				}

				lines.push_back(Vec4i(x1,y1,x2,y2));

			}
		}
	}

	//cout << "lines: " << lines.size() << " " << threshold << std::endl;
	return lines;
}
